#ifndef SEARCH_HPP
#define SEARCH_HPP

#include <queue>

#define NODE_VISITING   1
#define NODE_EXPANDED   2
#define NODE_SKIPPED    3

template <class T>
struct ignore_progress
{
    void operator()(int /* reason */, const T & /* state */) { }
};

template <class T>
struct search_stat
{
    size_t num_expanded;
    size_t num_visited;
    size_t num_skipped;
    void operator()(int reason, const T & /* state */)
    {
        switch (reason)
        {
        case NODE_VISITING: ++num_visited; break;
        case NODE_EXPANDED: ++num_expanded; break;
        case NODE_SKIPPED:  ++num_skipped; break;
        }
    }
};

template <class T>
struct astar_node
{
    T state;
    int g;      /* current cost */
    int f;      /* current cost + estimated remaining cost */

    astar_node(const T &_state, int current_cost, int estimated_cost) 
        : state(_state), g(current_cost), f(current_cost + estimated_cost)
    { }

    /* Returns true if this node is less promising than 'other'. */
    bool operator < (const astar_node<T> &other) const
    {
        return (f > other.f) || (f == other.f && g < other.g);
    }
};

template <class T, class Expander, class StateSet, class Heuristic, 
          class Progress>
int astar_search(
    const T &start,     /* Start state. */
    const T &goal,      /* Goal state. */
    StateSet &visited,  /* Set of states to be excluded from the search. */
    Expander expand,    /* Function that expands the children of a state. */
    Heuristic h,        /* Heuristic function. */
    Progress &report)   /* Search progress callback. */
{
    typedef astar_node<T> Node;
    std::priority_queue<Node> q;
    q.push(Node(start, 0, h(start)));
    while (!q.empty())
    {
        Node node = q.top();
        q.pop();

        const T &state = node.state;
        if (!visited.insert(state).second) /* already exists */
        {
            report(NODE_SKIPPED, state);
            continue;
        }

        report(NODE_VISITING, state);
        if (state == goal)
            return node.f;

        T child;
        for (size_t k = 0; expand(state, k, &child); k++)
        {
            if (visited.count(child) == 0)
            {
                q.push(Node(child, node.g + 1, h(child)));
                report(NODE_EXPANDED, child);
            }
        }
    }
    return -1;
}

#endif /* SEARCH_HPP */
